/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This is an effort to test if the Occupancygrid can be used for SLAMing
*/ 

#define MAP_FILE_NAME "../../res/autmap-strip.txt"
//#define MAP_FILE_NAME "../../res/autmap-koko.txt"

#include<stdio.h>
#include<stdlib.h>
#include<math.h>

#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation
#include "CTopoOccuMCL.h"
#include "CTopoScanOccuMCL.h"

#include "CScanMatchInterface.h" ///< Scanmatch interface
#include "CAHistogramECorrelationScanMatch.h" ///< Scanmatch 
#include "update_pose.h"

/// For reading the data from file

float *dist;
TOccupancyGrid *O;
int oNx,oNy;
vector<mcl::pose> godo;
vector<mcl::pose> odo;
TOccupancyGrid slamO(10.1,10.1,0.05,Grid::TCoord2D(0,0));

void initMapStuff(){
		O = new TOccupancyGrid(1,1,0.04,Grid::TCoord2D(0,0));
		
		//if(!O->loadLineMap("../../res/autmap-strip.txt", 1.0,1.0)){
		if(!O->loadLineMap(MAP_FILE_NAME, 1.0,1.0)){
				fprintf(stderr,"**** Unable to load the map\n");
				exit(1);
		}
		slamO.setGridToValue(0.5);
		//slamO.loadLineMap("../../res/autmap-strip.txt", 1.0,0.9);
		slamO.loadLineMap(MAP_FILE_NAME, 1.0,0.9);
		
		O->getDim(oNx, oNy);
		dist = (float *) malloc(oNx*oNy*sizeof(float));
		fprintf(stderr,"*** Computing distance transform ***\n");
		O->distanceTransform(dist, 1.0);	 
		fprintf(stderr,"Done!\n");
}

float computeCorrelation(mcl::scan &meas, mcl::pose &p,float maxdist){
		float x,y,corr=0;
		Grid::TCoord2D ocoord;
		Grid::TPixel pix;
		int n=0;
		for(int i=0;i<meas.N;i++){
				if(meas.r[i]<maxdist){
						ocoord.x = p.x + meas.r[i]*cos(meas.a[i]+p.a);
						ocoord.y = p.y + meas.r[i]*sin(meas.a[i]+p.a);
						pix = O->toPixel(ocoord);
						corr+=dist[pix.y*oNx+pix.x];
						n++;
				}
		}
		if(n<80){
				corr=1000000; 
		}else corr = (corr/(float)n);
		
		
		return corr;
}

float fineTunePose(mcl::scan &meas, mcl::pose &p, float maxdist, int maxIter, float stopTh){
		ownRandom rand;
		mcl::pose tmp;
		mcl::pose best = p;
		float corr, bestcorr=10000;
		
		bestcorr = computeCorrelation(meas,p,maxdist);
		int iter=0;
		
		while(iter<maxIter && bestcorr>stopTh){
				tmp.x = p.x + rand.normalRandom()*0.2;
				tmp.y = p.y + rand.normalRandom()*0.2;
				tmp.a = p.a + (rand.normalRandom()*M_PI/180)*3.0;
				corr = computeCorrelation(meas,tmp,maxdist);
				iter++;
				
				if(corr<bestcorr){
						best = tmp;
						bestcorr = corr;
						fprintf(stderr,"Better: %.3f\n",corr);
				}
		}
		if(iter>=maxIter){
				fprintf(stderr,"Reached maximum iteration number... quitting!\n");
		}
		else{
				fprintf(stderr,"Stop threshold reached!!! \n");
		}
		p = best;
		return bestcorr;
}

float gridTunePose(mcl::scan &meas, mcl::pose &p, float maxdist, int iter){
		mcl::pose tmp;
		mcl::pose best = p;
		float corr, bestcorr=10000;
		
		bestcorr = computeCorrelation(meas,p,maxdist);
		fprintf(stderr,"First: %.3f ",bestcorr);
		
		//if(bestcorr > 0.5) return bestcorr;
		
		//int iter=0;
		
		float x,y,a;
		float POS_SEARCH = 0.08, POS_RES=0.02;
		float ANG_SEARCH = 2.0*M_PI/180.0;
		float ANG_RES = M_PI/720.0;
		while(iter > 0){
				for(x=p.x-POS_SEARCH*(float)iter;x<p.x+POS_SEARCH*(float)iter;x+=POS_RES*(float)iter){
						for(y=p.y-POS_SEARCH*(float)iter;y<p.y+POS_SEARCH*(float)iter;y+=POS_RES*(float)iter){
								for(a=p.a-ANG_SEARCH*(float)iter;a<p.a+ANG_SEARCH*(float)iter;a+=ANG_RES*(float)iter){
										tmp.set(x,y,a);
										corr = computeCorrelation(meas,tmp,maxdist);
										if(corr<bestcorr){
												best = tmp;
												bestcorr = corr;
												//fprintf(stderr,"Better: %.3f\n",corr);
										}
								}///A
						}///Y
				}///X
				fprintf(stderr,"Best: %.3f\n",bestcorr);
				p = best;
				iter--;
		}
		return bestcorr;
}


static int lastCorrected=-1;
void odoCorrector(mcl::pose &p, int index){
		mcl::pose mest;
		mcl::pose corPos;
		
		if(lastCorrected == -1){
				lastCorrected = index;
				godo[index]=p;
				for(int i=index;i<godo.size()-1;i++){
						mest.setToDifferentialPose( odo[i+1],odo[i]);
						corPos = godo[i].integrateDifferential(mest);
						godo[i+1] = corPos;
				}
				for(int i=index;i>0;i--){
						mest.setToDifferentialPose( odo[i-1],odo[i]);
						corPos = godo[i].integrateDifferential(mest);
						godo[i-1] = corPos;
				}	
		}
		else{
				mcl::pose orig = godo[index];
				mcl::pose err = orig;
				int N = index-lastCorrected;
				int last = lastCorrected;
						
				err.x -= p.x; 
				err.y -= p.y; 
				err.a -= p.a; 
				
				if(err.a > M_PI){
						fprintf(stderr,"WARNING: Not good angle error!\n");
						while(err.a>M_PI) err.a-=2.0*M_PI; 
				}else if(err.a < -M_PI){
						fprintf(stderr,"WARNING: Not good angle error!\n");
						while(err.a < -M_PI) err.a+=2.0*M_PI; 
				}
				
				fprintf(stderr,"Orig_err (%.2f %.2f %.2f) ", err.x,err.y,err.a);
				err.a/=(float)(N);
				
				
				lastCorrected = index;
				godo[index]=p;
				for(int i=index;i<godo.size()-1;i++){
						mest.setToDifferentialPose( odo[i+1],odo[i]);
						corPos = godo[i].integrateDifferential(mest);
						godo[i+1] = corPos;
				}
				///Now compute the correction from the last correctio to present
				/// First the heading
				godo[index] = orig;
				for(int i=last;i<index;i++){
					  mest.setToDifferentialPose( godo[i+1],godo[i]);
						if(i>last) godo[i] = corPos; 
						mest.a -= err.a;
						corPos = godo[i].integrateDifferential(mest);
				}
				godo[index]=corPos;
			//	mest.setToDifferentialPose( godo[i+1],godo[i]);
			//	mest.a += err.a;
			//	corPos = godo[i].integrateDifferential(mest);
				
				
				
				///////// Virhe ....
				err = godo[index];
			 	err.x -= p.x; 
			 	err.y -= p.y; 
			 	err.a -= p.a; 
			 	fprintf(stderr,"Err_Now (%.2f %.2f %.2f) ", err.x,err.y,err.a);
				err.y/=(float)(N);
				err.x/=(float)(N);
				
				for(int i=last;i<=index;i++){
					godo[i].x -= (i-last)*err.x;
					godo[i].y -= (i-last)*err.y;
				}
				err = godo[index];
				err.x -= p.x; 
				err.y -= p.y; 
				err.a -= p.a; 
				fprintf(stderr,"Final (%.2f %.2f %.2f) ", err.x,err.y,err.a);
				
		}
}



int readScanLine181(FILE *f,mcl::scan &meas, mcl::pose &p){
		float time_s;
		int i;
		char c;
		fscanf(f,"%f %f %f",&p.x,&p.y,&p.a); ///< Pose
	//fprintf(stderr,"1 ");
		for(i=0;i<181;i++){
				fscanf(f,"%f %f",&meas.r[i],&meas.a[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<181){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"3 \n");
		meas.N = 181;
		return 0;
}

int readSilmuPoseData(FILE *f,mcl::scan &meas, mcl::pose &p){
		float t1,t2;
		int i;
		char c;
		fscanf(f,"%f %f %f %f %f %d\n",&t1,&p.x,&p.y,&t2,&p.a,&i); ///< Pose
		meas.N = 0;
		
		return 0;
}

 int readSilmuPoseDataNuppu(FILE *f,mcl::scan &meas, mcl::pose &p){
		float t1,t2,foo1,foo2;
		int i;
		char c;
		//fscanf(f,"%f %f %f %f %f %d %f %f\n",&t1,&p.x,&p.y,&t2,&p.a,&i,&foo1,&foo2); ///< Pose
		fscanf(f,"%f %f %f %f %f %d %f %f\n",&t1,&foo1,&foo2,&t2,&p.a,&i,&p.x,&p.y); ///< Pose
		meas.N = 0;
		
		return 0;
}


int readLaserOdometryData(FILE *f,mcl::scan &meas, mcl::pose &p){
		float p1,p2,p3;
		int i;
		char c;
		fscanf(f,"%f %f %f %f %f %f ", &p.x,&p.y,&p.a,&p1,&p2,&p3); ///< Pose
		for(i=0;i<361;i++){
				fscanf(f,"%f",&meas.r[i]);
				meas.a[i]=0.5*i*M_PI/180.0;
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<361){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
		meas.N = 361;
		return 0;

}



void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
	rgb[3*Nx*j+3*i]=r;
	rgb[3*Nx*j+3*i+1]=g;
	rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
	float *data = gr->getDataPtr();
	int Nx,Ny;
	gr->getDim(Nx,Ny);
	float v;
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
				v = data[Nx*j+i];
				//if(v>0.29) v = 0.9;
				setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
		}
	}	
}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
		for(int ii=i-1;ii<i+1;ii++){
				for(int jj=j-1;jj<j+1;jj++){
						setRGBpix(rgb,ii,jj,Nx,r,g,b);
				}
		}

}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
		float dy = sin(angle);
		float dx = cos(angle);
		float rr=0;
		int i=0;
		uint x,y;
		while(rr<length){
				x = pix + i*dx;
				y = piy - i*dy;
				RGB[3*Nx*y+3*x] = r;
				RGB[3*Nx*y+3*x+1] = g;
				RGB[3*Nx*y+3*x+2] = b;
				i++;
				rr+=resolution;
		}
}



int main(int argc, char *argv[]){
		float grid_size = 45.0;
		Grid::TCoord2D ocoord;
		Grid::TCoord2D coord;
		Grid::TPixel pix;
		CTopoOccuMCL topo;
		//CTopoScanOccuMCL topo;
		
		grid_t *origmap;
		grid_t *dataPtr;
		int Nx,Ny;
		unsigned char *rgb,*rgb2;
		
		if(argc != 2){
			fprintf(stderr,"\nUsage: ./map_matcher [dir]\n\n");
			fprintf(stderr,"[dir] - the directory where the laser_update.txt is located\n");
			return 0;
		}
		char *directory=argv[1];
		
		fprintf(stderr,"Using directory %s\n",directory);
		
		
		/////////////////////////////////////////////////////////////////////////////////////////////////
		//FILE *log = fopen("j2b2_measurement.txt","rt");
		//FILE *log = fopen("silmupose_data.txt","rt");
		
		
		initMapStuff(); ///< For correlation
		
		///For SLAM matching
		ScanMatch::scan meas,ref;  ///< The Reference scan 
		meas.allocate(361);
		ref.allocate(361);
		Grid::TScan virt; ///< for occupancy grid
		virt.alloc(361);
		CScanMatchInterface *gmatcher; ///< This is for global
	
		gmatcher = new CAHistogramECorrelationScanMatch(
																				.8,   // Search area in meters 
																				0.6);     // Search angle in radians
		
	/*
		topo.initialize("../../res/autmap-new.txt",
										1.0, 
										10000, 
										mcl::pose(-22,1,70*M_PI/180),  
										mcl::pose(0.1,0.1,M_PI/30),
										0.1,
										1.0);
		*/
		
		
		/**
		* Robo room!
		topo.initializeGlobal("../../res/autmap-new.txt",
													1.0, 
													0.1,  
			 										mcl::pose(0.4,0.4,M_PI/10), 
													mcl::pose(8,-8,0),	mcl::pose(15,0,2*M_PI));
		****/
		////////////////////////////////////////////////////////////////////////
		////////////////////////////////////////////////////////////////////////
		/// TOPO
		////////////////////////////////////////////////////////////////////////
		////////////////////////////////////////////////////////////////////////
		
		topo.initializeGlobal(MAP_FILE_NAME,
													1.0, 
													0.1,  
													mcl::pose(0.4,0.4,M_PI/20), 
													mcl::pose(-22,-2,0),	mcl::pose(-10,4,2*M_PI));
		
		
		////////////////////////////////////////////////////////////////////////
		////////////////////////////////////////////////////////////////////////
		/// TOPO-SCAN
		////////////////////////////////////////////////////////////////////////
		////////////////////////////////////////////////////////////////////////
		/*
		topo.initializeGlobal(MAP_FILE_NAME,
													1.0, 
													0.1,  
													mcl::pose(0.4,0.4,M_PI/20), 
													mcl::pose(-22,-2,0),	mcl::pose(-10,4,2*M_PI),
													5,0);
		*/
		
		
		if(chdir(directory)==-1){
			fprintf(stderr,"Invalid directory '%s'!\n",directory);
			return 0;
		}
		FILE *log = fopen("laser_update.txt","rt");
		if(log==NULL){
			fprintf(stderr,"No such file\n");	
			return 0;
		}
		
		
		
		topo.O->getDim(Nx,Ny);
		fprintf(stderr,"Map size = %dx%d\n",Nx,Ny);
		origmap = (grid_t *) malloc(Nx*Ny*sizeof(float));
		dataPtr = topo.O->getDataPtr();
		memcpy(origmap,dataPtr,Nx*Ny*sizeof(float));		
		
		///Initialize Image display
		//int slamNx,slamNy;
		//slamO.getDim(slamNx,slamNy);
		
		rgb = (unsigned char *) malloc(3*Nx*Ny);
		//rgb2 = (unsigned char *) malloc(3*slamNx*slamNy);
		
		addImageWindow(Nx, Ny);
		//addImageWindow(slamNx, slamNy);
		glutImageGUI_Start();
		
		mcl::scan s; ///< for occupancy grid
		s.allocate(361);
		mcl::pose odo_cur, odo_ref,mest;
		Grid::TScan gscan;
		gscan.alloc(361);
		//readScanLine181(log,s,odo_cur);
		//readSilmuPoseData(log,s,odo_cur);
		//readSilmuPoseDataNuppu(log,s,odo_cur);
		readLaserOdometryData(log,s,odo_ref);
		
		
		displayImage(0, rgb,Ny,Nx);
		
		long int ss0,us0,ss1,us1,ss2,us2,sec,us,ms;
		float fms;
		ownTime_GetTimeOfDay(&ss0,&us0);
		int cnt=0;
		
		
		vector<mcl::pose> path;
		vector<mcl::pose> path_back;
		vector<mcl::pose> cov_ahead;
		vector<mcl::pose> cov_back;
		vector<mcl::pose> cov_final;
		vector<float> path_var;
		vector<float> path_back_var;
		vector<mcl::scan> scans;
		
		odo.push_back(odo_ref);
		godo.push_back(odo_ref);
		scans.push_back(s);
		
		
		
 		while(!feof(log)){
				readLaserOdometryData(log,s,odo_cur);
				odo.push_back(odo_cur);
				godo.push_back(odo_cur);
				scans.push_back(s);
				//for(int i=0;i<1; i++) {
						//readScanLine181(log,s,odo_cur);
						//readSilmuPoseData(log,s,odo_cur);
						//readSilmuPoseDataNuppu(log,s,odo_cur);
				//}
				////////////////////////////////////////////////////////////////////////////
				/// The MCL
				////////////////////////////////////////////////////////////////////////////
				//mest.setToDifferentialPose(  *((mcl::pose *) &odo_cur),*((mcl::pose *) &odo_ref));
				mest.setToDifferentialPose( odo_cur,odo_ref);
				float calib=1.0;
				mest.x *= calib;
				mest.y *= calib; 
				//topo.runMCL(mest, mcl::pose(fabs(mest.x)*0.02+0.01,fabs(mest.y)*0.02+0.01,0.005*fabs(mest.a)),  s);
				
				topo.runMCL(mest, mcl::pose(fabs(mest.x)*0.2,fabs(mest.y)*0.2,0.001*fabs(mest.a)),  s);
				odo_ref = odo_cur;				
				mcl::pose mp = topo.filt.getDistributionMean(true); ///< The distribution mean
				mcl::pose var = topo.filt.getDistributionVariances();
				
				
				mp.toPI();
				//if(sqrt(var.x*var.x+var.y*var.y)<6.0)	
				path.push_back(mp);
				//var.a = var.a*180.0/M_PI/3.0;
				path_var.push_back(var.x*var.x+var.y*var.y+var.a*var.a);
				cov_ahead.push_back(mcl::pose(var));
				
				if(cnt%10==0){
						fprintf(stderr,"Est:(%.2f,%.2f,%.2f) ",mest.x,mest.y,mest.a);
						fprintf(stderr,"VAR: %.2f  %.2f %.2f\n",var.x,var.y,var.a);
				}
				//fprintf(stderr,"Lik=%.3f g_o:(%.2f,%.2f,%.2f) mp:(%.2f,%.2f,%.2f)\n",
				//				omcl.filt.Lik, g_origin.x,g_origin.y,g_origin.a,
				//			 mp.x,mp.y,mp.a);
				
			
				////////////////////////////////////////////////////////////////////////////
				/// Visualisation
				////////////////////////////////////////////////////////////////////////////
				
				
				if(cnt%40==0){
					//fprintf(stdout,"Drawing");
					setRGBmap(rgb,topo.O, 200,255, 200);
					Grid::TCoord2D coord;
					Grid::TPixel pix;
					for(int i=0;i<topo.filt.NumOfParticles;i++){
						coord.x = topo.filt.Particles[i].x;
						coord.y = topo.filt.Particles[i].y;
						pix = topo.O->toPixel(coord);
						setRGBpix(rgb,pix.x,pix.y, Nx, 255,255, 0);
					}
					
					Grid::TCoord2D ocoord;
					ocoord.x = mp.x;ocoord.y = mp.y;
					pix = topo.O->toPixel(ocoord);
					drawLine(rgb, Nx,Ny, topo.O->getResolution(),pix.x,pix.y,mp.a, 2.0,255,0,0);
					drawLine(rgb, Nx,Ny, topo.O->getResolution(),pix.x,pix.y,mp.a+M_PI/2, 2.0,0,255,0);
					
					for(int i=0;i<path.size();i+=1){
							ocoord.x = path[i].x;ocoord.y = path[i].y;
							pix = topo.O->toPixel(ocoord);
							setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0 );
					}
					displayImage(0, rgb,Ny,Nx);
					
				}
				////////////////////////////////////////////////////////////////////////////
				/// The end
				////////////////////////////////////////////////////////////////////////////	
				cnt++;
		}
		cnt = 0;
		for(int i=odo.size();i>1;i--){
				cnt++;
				odo_cur=odo[i-2];
				odo_ref = odo[i-1];
				mest.setToDifferentialPose( odo_cur,odo_ref);
				
				topo.runMCL(mest, mcl::pose(fabs(mest.x)*0.3,fabs(mest.y)*0.3,0.001*fabs(mest.a)),  s);
				odo_ref = odo_cur;
				
				mcl::pose mp = topo.filt.getDistributionMean(true); ///< The distribution mean
				mcl::pose var = topo.filt.getDistributionVariances();
				mp.toPI();
				
				path_back.push_back(mp);
				//var.a = var.a*180.0/M_PI/3.0;
				path_back_var.push_back(var.x*var.x+var.y*var.y+var.a*var.a);
				cov_back.push_back(var);
				if(cnt%10==0){
						fprintf(stderr,"Est:(%.2f,%.2f,%.2f) ",mest.x,mest.y,mest.a);
						fprintf(stderr,"VAR: %.2f  %.2f %.2f\n",var.x,var.y,var.a);
				}
				
				if(cnt%40==0){
					//fprintf(stdout,"Drawing");
						setRGBmap(rgb,topo.O, 200,255, 200);
						Grid::TCoord2D coord;
						Grid::TPixel pix;
						for(int i=0;i<topo.filt.NumOfParticles;i++){
								coord.x = topo.filt.Particles[i].x;
								coord.y = topo.filt.Particles[i].y;
								pix = topo.O->toPixel(coord);
								setRGBpix(rgb,pix.x,pix.y, Nx, 255,255, 0);
						}
					
						Grid::TCoord2D ocoord;
						ocoord.x = mp.x;ocoord.y = mp.y;
						pix = topo.O->toPixel(ocoord);
						drawLine(rgb, Nx,Ny, topo.O->getResolution(),pix.x,pix.y,mp.a, 2.0,255,0,0);
						drawLine(rgb, Nx,Ny, topo.O->getResolution(),pix.x,pix.y,mp.a+M_PI/2, 2.0,0,255,0);
					
						for(int i=0;i<path_back.size();i+=1){
								ocoord.x = path_back[i].x;ocoord.y = path_back[i].y;
								pix = topo.O->toPixel(ocoord);
								setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0 );
						}
						displayImage(0, rgb,Ny,Nx);
					
				}
		}
		
		vector<mcl::pose> path_final;
		vector<float> var_final;
		vector<int> indexes;
		FILE *ftopo=fopen("topo_path.txt","wt");
		for(int i=0;i<path.size();i++){
				if((path_var[i]<path_back_var[path.size()-1-i])){
						path_final.push_back(path[i]);
						var_final.push_back(path_var[i]);
						cov_final.push_back(cov_ahead[i]);
				}
				else{
						path_final.push_back(path_back[path.size()-1-i]);
						var_final.push_back(path_back_var[path.size()-1-i]);
						cov_final.push_back(cov_back[path.size()-1-i]);
				}
				indexes.push_back(i);
				fprintf(ftopo,"%.3f %.3f %.3f %.3f %.3f %.3f\n",path_final[i].x,
																												path_final[i].y,
																												path_final[i].a,
																												cov_final[i].x,cov_final[i].y,cov_final[i].a);
				
		}
		fclose(ftopo); ///< Topo path saved
	
		
		fprintf(stderr,"Running the pose estimator\n");
		UpdatePose estimator;
		
		vector<mcl::pose> topo_pose;
		vector<mcl::pose> odo_pose;
		topo_pose = path_final;
		odo_pose = odo;
		
		odoCorrector(path_final[0], 0);
		odo = godo;
		
		estimator.initialize(odo,4);
		//fprintf(stderr,"Exiting...\n");
		//return 0;
		estimator.update(path_final, indexes, cov_final);
		
		for(int i=0;i<3;i++){
				fprintf(stderr,"Iteration %d\n",i+1);
				
				for(int i=0;i<path_final.size();i++){
						
						odo[i].set(estimator.Xpos[3*i],estimator.Xpos[3*i+1],estimator.Xpos[3*i+2]);
				}
				setRGBmap(rgb,topo.O, 200,255, 200);
				for(int i=0;i<path_final.size();i+=1){
						ocoord.x = path_final[i].x;ocoord.y = path_final[i].y;
						pix = topo.O->toPixel(ocoord);
						setRGBpixBold(rgb,pix.x,pix.y, Nx, 0,255, 0 );
						
						ocoord.x = odo[i].x;ocoord.y = odo[i].y;
						pix = topo.O->toPixel(ocoord);
						setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0 );
						
				}
				
				displayImage(0, rgb,Ny,Nx);
				fprintf(stderr,"here 1\n");
				estimator.iterate();
				fprintf(stderr,"here 2\n");
				usleep(1000*1000);
					
		}
		for(int i=0;i<path_final.size();i++){
				path_final[i].set(estimator.Xpos[3*i],estimator.Xpos[3*i+1],estimator.Xpos[3*i+2]);
		}
		FILE *ffinal=fopen("topo_odo.txt","wt");
		for(int i=0;i<path_final.size();i++){
			fprintf(ffinal,"%f %f %f\n",path_final[i].x,path_final[i].y,path_final[i].a);
		}
		fflush(ffinal);
		fclose(ffinal);
		
		odo = odo_pose;
		
		
	  fprintf(stderr,"The end - press CTRL-C\n");
	  while(1){
			usleep(1000*1000);	
				setRGBmap(rgb,topo.O, 200,255, 200);
				for(int i=0;i<path_final.size();i+=1){
						ocoord.x = path_final[i].x;ocoord.y = path_final[i].y;
						pix = topo.O->toPixel(ocoord);
						setRGBpixBold(rgb,pix.x,pix.y, Nx, 0,255, 0 );
						
						//ocoord.x = odo[i].x;ocoord.y = odo[i].y;
						//pix = topo.O->toPixel(ocoord);
						//setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0 );
						
						ocoord.x = topo_pose[i].x;ocoord.y= topo_pose[i].y;
						pix = topo.O->toPixel(ocoord);
						setRGBpixBold(rgb,pix.x,pix.y, Nx, 0,0, 255 );
						
						//ocoord.x = odo_pose[i].x;ocoord.y= odo_pose[i].y;
						//pix = topo.O->toPixel(ocoord);
						//setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,255, 255 );
						
						ocoord.x = godo[i].x;ocoord.y= godo[i].y;
						pix = topo.O->toPixel(ocoord);
						setRGBpixBold(rgb,pix.x,pix.y, Nx, 128,128, 128 );
						
						
				}
				
				displayImage(0, rgb,Ny,Nx);	
		}
		
		//cov_ahead.clear(); cov_back.clear();
		//cov_final.clear(); var_final.clear();
		
		vector<float> path_corr;
		/**
		* Compute correlation for each topological pose 
		**/
		fprintf(stderr,"Computing the correlation for whole data set (%d pose, %d scan)\n",
						path_final.size(), scans.size());
		for(int i=0;i<path_final.size();i+=1){
				float corrr= computeCorrelation(scans[i], path_final[i],16.0);
				fprintf(stderr,"%f\n",corrr);
				path_corr.push_back(corrr);
				if(i%250==0) fprintf(stderr,"%.2f completed\n",100.0*(float)(i)/(float)path_final.size()); 
		}
		
		
		
		
		
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss0;us = us2-us0; ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		fprintf(stderr,"TOTAL::(%.4f) = (%.4f)/scan \n",fms,fms/cnt);
		
		fprintf(stderr,"The End - press ctrl C to stop\n");
		cnt=0;
		int draw = 0;
		
		int cor_cnt=0;
		while(1){
			if(cnt%100==0) draw = 1;
			
			if(cnt<scans.size()){
				//if(var_final[cnt]<.3){
				if(path_corr[cnt]<0.3){
						mcl::pose tune =  path_final[cnt];
						//if(fineTunePose(scans[cnt],tune , 8.0, 10000, 0.01)<0.1){
						if(gridTunePose(scans[cnt],tune , 16.0,4)<0.1){
								path_final[cnt] = tune;
								
								for(int j=0;j<scans[cnt].N;j++){
										gscan.r[j] = scans[cnt].r[j];
										if(gscan.r[j] > 16.0) gscan.r[j] = 0; 
										gscan.a[j] = scans[cnt].a[j];
								}
								topo.O->setScanFillProb(path_final[cnt].x, path_final[cnt].y,
																								path_final[cnt].a, gscan,0.3f,0.9f);
								
								odoCorrector(tune, cnt);
								draw = 1;
						}
				}
			}else{
					if(cor_cnt == 0){
							FILE *fodo = fopen("corrected_odo.txt","wt");
							lastCorrected = -1;
							for(int i=0;i<odo.size();i++){
								odo[i] = godo[i];
								fprintf(fodo,"%.3f %.3f %.3f\n",odo[i].x,odo[i].y,odo[i].a);
							}
							fclose(fodo);
					}
					if(0){
					/****************************************************************************
					///Processing completed - time to fit the odometry to the map
					if(cor_cnt<scans.size()){
							mcl::pose tune =  odo[cor_cnt];
							
							slamO.getScan(tune.x,tune.y, tune.a, 361, M_PI/360.0, (grid_t) 0.8, virt);
							int badval=0;
							for(int i=0;i<virt.N;i++){
									ref.a[i]=virt.a[i];
									ref.r[i]=virt.r[i]; 
									if (virt.r[i]<0){ref.r[i]=0;badval++;}
							}
							ScanMatch::pose pos;
							pos.set(tune.x,tune.y,tune.a);
							for(int j=0;j<scans[cor_cnt].N;j++){
										meas.r[j] = scans[cor_cnt].r[j];
										if(meas.r[j] > 32.0) gscan.r[j] = 0; 
										meas.a[j] = scans[cor_cnt].a[j];
										gscan.r[j] = scans[cor_cnt].r[j];
										if(gscan.r[j] > 16.0) gscan.r[j] = 0; 
										gscan.a[j] = scans[cor_cnt].a[j];
								}
							
							if(badval < 30){
									gmatcher->setCurrentPose(pos);
									gmatcher->setReferenceScan(ref,pos); 
									gmatcher->update(meas,pos);    ///update
									gmatcher->getCurrentPose(pos); ///< Get the estimate
							}
							slamO.setScanFillProb(pos.x, pos.y ,pos.a, gscan,0.3,0.7);
							***********************************************************************/
							
// 							mcl::pose tune =  odo[cor_cnt];
// 							if(gridTunePose(scans[cor_cnt],tune , 16.0,4)<0.2){
// 									for(int j=0;j<scans[cor_cnt].N;j++){
// 											gscan.r[j] = scans[cor_cnt].r[j];
// 											if(gscan.r[j] > 16.0) gscan.r[j] = 0; 
// 											gscan.a[j] = scans[cor_cnt].a[j];
// 									}
// 									topo.O->setScanFillProb(tune.x, tune.y,
// 																					tune.a, gscan,0.3f,0.9f);
// 									odoCorrector(tune, cor_cnt);
// 									path_final[cor_cnt] = tune;
// 									draw = 1;
// 							}

					}else{
							if(cnt%10==0) fprintf(stderr,"No more scans!\n");
							usleep(1000*500);
							draw = 1;
					}
					cor_cnt++;		
			}
			
			
			
			if(draw){
					setRGBmap(rgb,topo.O, 200,255, 200);
					
				//if(cor_cnt>0) setRGBmap(rgb2,&slamO, 200,255, 200);
					
				
					for(int i=0;i<topo.filt.NumOfParticles;i++){
							coord.x = topo.filt.Particles[i].x;
							coord.y = topo.filt.Particles[i].y;
							pix = topo.O->toPixel(coord);
							setRGBpix(rgb,pix.x,pix.y, Nx, 255,255, 0);
					}
			}///IF DRAW
			
			if(draw && cnt<scans.size()){
					float x,y;
					for(int j=0;j<scans[cnt].N;j++){
							x = 2*gscan.r[j]*cos(gscan.a[j]);
							y = 2*gscan.r[j]*sin(gscan.a[j])-30;
							ocoord.x = x; ocoord.y=y;
							pix = topo.O->toPixel(ocoord);
							setRGBpixBold(rgb,pix.x,pix.y, Nx, 0,0, 255 );
// 							ocoord.x = path_final[cnt].x + gscan.r[j]*cos(gscan.a[j]+path_final[cnt].a);
// 							ocoord.y = path_final[cnt].y + gscan.r[j]*sin(gscan.a[j]+path_final[cnt].a);
// 							pix = topo.O->toPixel(ocoord);
// 							setRGBpixBold(rgb,pix.x,pix.y, Nx, 0,255, 255 );
					}
					
					for(int j=0;j<scans[cnt].N;j++){
							if(scans[cnt].r[j]>8.0)	gscan.r[j]=0; 
							else gscan.r[j]= scans[cnt].r[j];
							gscan.a[j]=scans[cnt].a[j];
										
							ocoord.x = path_final[cnt].x + gscan.r[j]*cos(gscan.a[j]+path_final[cnt].a);
							ocoord.y = path_final[cnt].y + gscan.r[j]*sin(gscan.a[j]+path_final[cnt].a);
							pix = topo.O->toPixel(ocoord);
							setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 255 );
					}
			}///IF DRAW
			
			if(draw){
				for(int i=0;i<path_final.size();i+=1){
						ocoord.x = path_final[i].x;ocoord.y = path_final[i].y;
						pix = topo.O->toPixel(ocoord);
						//setRGBpix(rgb,pix.x,pix.y, Nx, 255,0, 0 );
						if(var_final[i]<1000.0){
								setRGBpixBold(rgb,pix.x,pix.y, Nx, 0,255, 0 );
						}else if(var_final[i]<4.0){
								setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,255, 0 );
						}else{
								setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0 );
						}
				}
				for(int i=0;i<godo.size();i+=1){
						ocoord.x = godo[i].x;ocoord.y = godo[i].y;
						pix = topo.O->toPixel(ocoord);
						setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,255, 255 );
				}
				displayImage(0, rgb,Ny,Nx);
				//if(cor_cnt>0) displayImage(1, rgb2,slamNy,slamNx);
				draw = 0;
				usleep(1000);
			}
			cnt++;
		}
		return 0;
}